#include "sensor.h"

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/time.h>
#include "ringbuf.h"

#include "mpi_venc.h"
#include "hi_common.h"
#include "hi_comm_video.h"
#include "hi_comm_sys.h"
#include "mpi_sys.h"
#include "hi_comm_vb.h"
#include "mpi_vb.h"
#include "hi_comm_vpss.h"
#include "mpi_vpss.h"
#include "mpi_vgs.h"

#include "util.h"

#define SENSOR_ERROR_LIMIT 250

typedef struct SensorMgr {
	int fps;
	int height;
	int width;
	int frame_size;
	int vpssgrp;
	int vpsschext;
	int vpsschbind;
	pthread_t thr_id;
    int run;
    ringbuf_t rb;
}SensorMgr;

static SensorMgr sensormgr;

static int sensor_init();

static int sensor_start_grab();

HI_S32 sensor_start_vpss_chn(unsigned int vpssgroup,unsigned int vpsschext,unsigned int bindvpssch,unsigned int width,unsigned int height)
{
    HI_S32 s32Ret = HI_SUCCESS;
	
	VPSS_EXT_CHN_ATTR_S stVpssExtChnAttr;
	memset(&stVpssExtChnAttr, 0, sizeof(VPSS_EXT_CHN_ATTR_S));
	stVpssExtChnAttr.s32BindChn = bindvpssch;
	stVpssExtChnAttr.u32Width = sensormgr.width;
	stVpssExtChnAttr.u32Height = sensormgr.height;
	
	__log_mesg("Sensor width=%d height=%d \n",stVpssExtChnAttr.u32Width, stVpssExtChnAttr.u32Height);
    stVpssExtChnAttr.enPixelFormat  = PIXEL_FORMAT_YVU_SEMIPLANAR_420;
    stVpssExtChnAttr.enCompressMode = COMPRESS_MODE_NONE;
	stVpssExtChnAttr.enVideoFormat = VIDEO_FORMAT_LINEAR;
	stVpssExtChnAttr.u32Depth = 1;
	stVpssExtChnAttr.stFrameRate.s32SrcFrameRate = -1;
	stVpssExtChnAttr.stFrameRate.s32DstFrameRate = -1;    
	stVpssExtChnAttr.enDynamicRange = DYNAMIC_RANGE_SDR8;
    s32Ret = HI_MPI_VPSS_SetExtChnAttr(vpssgroup, vpsschext, &stVpssExtChnAttr);
    if (s32Ret != HI_SUCCESS)
    {
    	__log_mesg("HI_MPI_VPSS_SetChnAttr fail [%#x]!\n",s32Ret);
        return s32Ret;
    }

    s32Ret = HI_MPI_VPSS_EnableChn(vpssgroup,vpsschext);
    if (s32Ret != HI_SUCCESS)
    {
    	__log_mesg("HI_MPI_VPSS_EnableChn fail!\n");
        return s32Ret;
    }
	
    return HI_SUCCESS;
}

HI_S32 sensor_release_vpss_chn(unsigned int vpssgrp,unsigned int vpssch)
{
    HI_S32 s32Ret = HI_SUCCESS;

	s32Ret = HI_MPI_VPSS_DisableChn(0, vpssch);
	if (s32Ret != HI_SUCCESS)
    {
    	__log_mesg("HI_MPI_VPSS_DisableChn fail!\n");
        return s32Ret;
    }
	return 0;
}

int sensor_init()
{
    int ret = 0;
	sensormgr.width = 1920;
	sensormgr.height = 1080;
	sensormgr.frame_size = 1920 * 1080 * 3/2;
	sensormgr.vpssgrp = 0;
	sensormgr.vpsschbind = 3;
	sensormgr.vpsschext= 4;
    sensormgr.fps = 25;
	ret = sensor_start_vpss_chn(sensormgr.vpssgrp,
                              sensormgr.vpsschext,
                              sensormgr.vpsschbind,
                              sensormgr.width,
                              sensormgr.height);
    if(ret != 0) {
        __log_mesg("start_vpss_chn stream_thr error\n");
        return EXIT_FAILURE;
	}
    sensormgr.rb = ringbuf_new(8 * sensormgr.frame_size);
    return EXIT_SUCCESS;
}

int sensor_push_to_ringbuf(VIDEO_FRAME_INFO_S* pframe)
{
    char* head = 0;
    /**
     * u64VirAddr array declared as a long long variable
     * so u64VirAddr is differ from u64VirAddr[0]
     */
    head = (char*)ringbuf_memcpy_into(  sensormgr.rb,
                                        ((unsigned char*)pframe->stVFrame.u64VirAddr[0]),
                                        sensormgr.frame_size);
    return head?  EXIT_SUCCESS:EXIT_FAILURE;
}

void write_to_file(char* data , int len) {
    FILE* fp = fopen("raw.yuv", "a+");
    if (fp) {
        int nwrite = fwrite(data, 1, len, fp);
        if (nwrite != len) {
            __log_mesg("short write.write=%d,need=%d\n", nwrite, len);
        }
        fclose(fp);
    }
}

HI_S32 Public_yuv_save(VIDEO_FRAME_INFO_S *pFrame)
{
	static int count = 0;
	char acFile[128]  = {0};
    FILE *pFile = NULL;

    sprintf(acFile, "/var/tmp/123%d_%d_%d.yuv", count,pFrame->stVFrame.u32Width,pFrame->stVFrame.u32Height);
    pFile = fopen(acFile, "wb");
    if (pFile == NULL)
    {
        printf("open file err\n");
        return HI_FAILURE;
    }
	fwrite((char *)pFrame->stVFrame.u64VirAddr[0],1,pFrame->stVFrame.u32Width * pFrame->stVFrame.u32Height,pFile);
	fwrite((char *)pFrame->stVFrame.u64VirAddr[1],1,pFrame->stVFrame.u32Width * pFrame->stVFrame.u32Height >> 1,pFile);
	fclose(pFile);
	pFile = NULL;
	count++;
    return HI_SUCCESS;
}

void * sensor_grab_loop(void *arg)
{
	int ret = -1;
	struct timeval time_v = {0};
	unsigned long long frame_interval = 40; // 40 ms
	unsigned long long send_pts = 0;
	unsigned long long prev_send_pts = 0;

	if(sensormgr.fps  != 0)
	{
		frame_interval = 1000 / sensormgr.fps ;
	}
    
	//time_t now_time = 0;
	//struct tm now_tm = {};

	//now_time = time(NULL);
	//localtime_r(&now_time, &now_tm);

	int frame_id = 0;
	//VIDEO_FRAME_INFO_S stVFrame;
    //HI_MPI_VB_MmapPool(stVFrame.u32PoolId);
    int hasmapped = 0;
	while(sensormgr.run)
	{
		gettimeofday(&time_v, NULL);
		send_pts = time_v.tv_sec;
		send_pts = send_pts * 1000 + time_v.tv_usec / 1000;

		//if(send_pts / 1000 == prev_send_pts / 1000)
		//{
		//	frame_rate++;
		//}
		//else
		//{
		//	frame_rate=0;
		//}

        /* 因为时间会后退（这个可能是时间同步的原因）
        *  所以某一时刻send_pts可能小于prev_send_pts
        *  设send_pts=100 prev_send_pts=300
        *  则，sleep时间= 300+40-100 = 240,这会导致程序在这阻塞住。
        */
		if(send_pts < (prev_send_pts + frame_interval) && send_pts >= prev_send_pts)
		{
            int64_t sleeptime = (prev_send_pts+frame_interval-send_pts);
            //__log_mesg("sleep in sensor loop => %ld-ms\n", sleeptime);
			usleep(sleeptime*1000);  
		}
		
		
		VIDEO_FRAME_INFO_S stVFrame;
		ret = HI_MPI_VPSS_GetChnFrame(sensormgr.vpssgrp, sensormgr.vpsschext, &stVFrame, 200);
		if (ret != 0)
		{	
			__log_mesg("HI_MPI_VPSS_GetChnFrame error,ret=%#x\n",ret);
			//usleep(5 * 1000);
			continue;
		}
		
		HI_VOID *pVirAddr = NULL;
		if(stVFrame.stVFrame.u64VirAddr[0] == 0)
		{
            if (hasmapped == 0) {
                HI_MPI_VB_MmapPool(stVFrame.u32PoolId);
                hasmapped = 1;
            }
			
			HI_MPI_VB_GetBlockVirAddr(stVFrame.u32PoolId,stVFrame.stVFrame.u64PhyAddr[0],&pVirAddr);	
			if(pVirAddr == NULL)
			{
                __log_mesg("HI_MPI_VB_GetBlockVirAddr error, continue.\n");
				HI_MPI_VPSS_ReleaseChnFrame(sensormgr.vpssgrp, sensormgr.vpsschext, &stVFrame);
				//usleep(5*1000);
				continue;
			}
			
			stVFrame.stVFrame.u64VirAddr[0] = (HI_U64)pVirAddr;

            pVirAddr = NULL;			
			HI_MPI_VB_GetBlockVirAddr(stVFrame.u32PoolId,stVFrame.stVFrame.u64PhyAddr[1],&pVirAddr);			
			stVFrame.stVFrame.u64VirAddr[1] = (HI_U64)pVirAddr;

			pVirAddr = NULL;
			HI_MPI_VB_GetBlockVirAddr(stVFrame.u32PoolId,stVFrame.stVFrame.u64PhyAddr[2],&pVirAddr);			
			stVFrame.stVFrame.u64VirAddr[2] = (HI_U64)pVirAddr;
		}
		
		frame_id++;
        //usleep(2);
        //Public_yuv_save(&stVFrame);
        if (sensor_push_to_ringbuf(&stVFrame)) {
            // 图像缓冲区满，消费慢，生产快。 这里直接丢弃的处理方法，会产生跳帧现象
            //__log_mesg("Copy image to ringbuf failed. Algo cost less than 40ms and this should not hanppen.\n");

        }
        
        
		//__log_mesg("send one frame: pts = %lld, prev_send_pts=%lld, frame_rate=%d frame_id:%d!!!\n",
		//									send_pts, prev_send_pts, frame_rate,frame_id);
		prev_send_pts = send_pts;

		HI_MPI_VPSS_ReleaseChnFrame(sensormgr.vpssgrp, sensormgr.vpsschext, &stVFrame);
	}
    sensormgr.run = 0;
	return NULL;
}

int sensor_start_grab()
{
    int ret = -1;
    if (sensormgr.run) {
	    __log_mesg("Warn, sensor thread already started.\n");
	} else {
        sensormgr.run = 1;
        ret = pthread_create(&sensormgr.thr_id, 0, sensor_grab_loop, 0);
        if(ret)
        {
            sensormgr.run = 0;
            __log_mesg("pthread_create stream_thr error\n");
        	return EXIT_FAILURE;
        }
    }
    return EXIT_SUCCESS;  
}

int sensor_close()
{
    int ret = 0;
    sensormgr.run = 0;  
    if(sensormgr.thr_id)
	{
        ret = pthread_join(sensormgr.thr_id, NULL);
		if (ret)
		{
			__log_mesg("stream_thr pthread_join error: ret=%d !!!\n", ret);
			//goto Err;
		}
	}
    sensor_release_vpss_chn(sensormgr.vpssgrp, sensormgr.vpsschext);
 //Err:   	
    ringbuf_free(sensormgr.rb);
    return EXIT_SUCCESS;
}

int sensor_get_fps()
{
    return sensormgr.fps;
}

int sensor_open()
{
    if (sensor_init()) {
        return EXIT_FAILURE;
    }
    if (sensor_start_grab()) {
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

int sensor_get_width()
{
    return sensormgr.width;
}

int sensor_get_height()
{
    return sensormgr.height;
}

int sensor_get_image(unsigned char** data, int* pwidth, int* pheight)
{
    *pwidth = sensormgr.width;
    *pheight = sensormgr.height;
    *data = (unsigned char*)malloc(sensormgr.frame_size);
    char* tail = 0;
    tail = (char*)ringbuf_memcpy_from(*data, sensormgr.rb, sensormgr.frame_size);
    if (tail == 0) {
        free(*data);
    }
    return tail? EXIT_SUCCESS : EXIT_FAILURE;
}

void sensor_free_image(unsigned char* data)
{
    if (data) {
        free(data);
        data = 0;
    }
}

void test_get_sensor_image()
{
    unsigned char* data = 0;
    int width = 0;
    int height = 0;
    int ret = sensor_get_image(&data, &width, &height);
    if (ret == EXIT_SUCCESS) {
        __log_mesg("get image failed.\n");
    } else {    
        __log_mesg("get image done. ==> w=%d h=%d data=%p\n", width, height, data);
    }
    usleep(25000);
    sensor_free_image(data);
}

void sensor_test()
{
    char buf[64]={0};
    int i = 0;
    while(1)
    {
        fflush(stdout);
        memset(buf, 0, sizeof(buf));

        for (i = 0; i < 64; i++)
        { 
            buf[i]=getchar();
            if(buf[i] == '\n')
            {
                //printf("\n");
                break;
            }            
        }

        // 命令长度超出，丢弃
        if(i >= 64)
        {
            ///避免串口作为其他用途时，getchar总是立即返回构成死循环
            usleep(1000*1000);
            continue;
        }
    
        // 去掉多余的换行符
        if(i > 0 && buf[i - 1] == '\r')
        {
            i--;
        }
        buf[i] = '\0';

        switch(buf[0])
        {
            case 'o':
                if (sensor_open()) {
                    __log_mesg("sensor_open failed.\n");
                }   
                break;
            case  'c':
                if (sensor_close()) {
                    __log_mesg("gige_close failed.\n");
                }
                break;
            case 'i':
                test_get_sensor_image();
                break;
            default:
                break;
        }
    }
}
